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ABSTRACT . 


Optimal collision , - free path planning for 
robot manipulators has been attempted utilizing the method of 
Local Variations (MLV). Methods for checking collision 
between robot arm and static, as well as dynamic, obstacles 
have been developed. Collision is detected by checking for 
geometric intersection of two objects in the workspace of the 
robot. This scheme has been incorporated in the MLV 
algorithm and used to solve the minimum - energy path 
planning problem for a robot in the presence of static 
obstacles. This approach has also been applied to solve the 
minimum - energy and minimum - time path planning problems 
for two robots operating simultaneously in the same workspace. 

A scheme for determining the changes in energy 
with variation in traversal time has been developed. This 
scheme is utilized to determine the minimum - time - energy 
geometric path for a single robot. All these methods were 
implemented on the DEC - 1090 mainframe computer and the 
results verified through computer simulation of the first 
three joints of PUMA - 560 manipulator. 
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CHAPTER 1 . 


INTRODUCTION. 


The requirement of consistent quality 
together with high productivity has led to rapid increase in 
the use of robotic manipulators in industry. An industrial 
robot is a multifunctional programmable manipulator which 
can perform flexible tasks. Robot systems are capable of 
performing various tasks requiring high degree of 
dexterity and adaptability to various environments with 
relatively low costs. 


Uith increasing stress on productivity 
and optimum utilization of resources, task planning and 
control of robotic manipulators attain significance. Some of 
the factors to be considered during task planning are 
the energy consumption and the time of operation. Further, 
there may be a number of static, as well as dynamic obstacles 
in the workspace of the robot. The control of a robotic 
manipulator should be such that it drives the robot arm 
along a path free from collisions while consuming minimum 
amount of energy or traveling in a minimum time. 

1.1. Path Planning And Control : 

Robots must be able to work in large 
crowded spaces, handle a variety of work pieces and perform 
flexible tasks. The dynamic behavior of robotic manipulators 
is highly complex, since the dynamics of multi-input, 
multi-output spatial linkages are highly coupled and 


nonl inear . 
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The kinematic and dynamic complexities 
create an unique control problem that cannot be adequately 
handled by standard linear control techniques. This makes 
effective control system design a critical and exciting 
issue in robotics. 

Further, robots are required to interact 
much more heavily with peripheral devices than 
traditional numerically controlled machine tools. These 
machine tools are essentially self contained systems that 
handle work pieces in veil defined locations. By contrast, 
the environment in which robots are used is often poorly 
structured and effective means must be developed to 
identify the locations of workpieces as well as to 
communicate with peripheral devices and other machines in a 
coord inat ed fashion . 

In general, it is extremely difficult to 
obtain an exact closed-form solution to the optimal control 
problem of robots mainly due to the following reasons: 

(I) the non-linearity and coupling in the 
manipulator dynamics and 

(ii) the complexity involved in collision 
avoidance computations . 

Due to this, the optimal control problem is usually 
divided into two subproblems, i.e. trajectory planning and 
trajectory tracking. This division can be easily explained 
by Fig 1,1. [1] 

Thus, the optimal control problem is 
to determine the controls which will drive the manipulator 
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from a given initial configuration to a given final 
configuration under the actuator and positional 
constraints, while minimizing a preselected cost function. 

1.2. Earlier Approaches To Path Planning : 

A number of model based 
manipulator programming problems have been addressed 
independent of any manipulator system, especially the 
problem of collision detection and collision avoidance 
among obstacles. The earliest algorithms first formulated 
the problem of collision avoidance In terms of an obstacle 
transformation which allows the moving object to be treated 
as a point. This is termed as the configuration space 
approach. [2 -5].. Conceptually, the configuration space 
approach may be viewed as shrinking the object to a point 
while at the same time expanding the obstacles to the shape 
of the moving object. This approach works well when the 
moving object is not allowed to rotate. If it rotates, then 
the grown obstacle must be embedded in a higher dimensional 
space - one dimension for each degree of rotational freedom. 
Furthermore , the grown obstacles will have nonplanar surfaces 
even when the original surface was polygonal or even 
polyhedral. This problem was partly overcome by slice 
projection method [6]. Here the rotation range is split 
into a fixed number of slices and within each slice the 
object is bounded by a polyhedra. 

In the Visibility graph method [2,4], 
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the robot is represented as a point and the obstacles 
are suitably grown. From the initial point on the path, the 
final point is viewed. If this point is visible, it 

indicates the presence of col 1 i s i on- f ree path which is a 
straight line joining the initial and final points of- the 
desired robot motion. If the goal is not visible, the robot 
is moved to the nearest vertex of the obstacle, which is 
lying on the line joining the initial and final points of 
the desired path. The robot moves along one of the edges of 
the obstacle till it reaches another vertex or till the 
goal is visible. This method, usually used in navigation, is 
used to compile the visibility graph. Collision-free 

trajectories can be developed by searching the visibility 

graph. 

Both the configuration space and 

visibility graph approaches deal with the problem of find 
space and find path, i.e. problems that involve finding 

where to place or how to move a solid in the presence of 

obstacles . 

Boyce [7] presented a structure for 
representing three dimensional objects and an algorithm for 
detecting intersections and collisions among these 
objects. Three basic object types are represented, viz. 

surfaces, solids and containers. To simplify interference 
checking computations, computer representations are limited 
to polyhedra. Static interference detection is done by 
checking for surface intersections, while dynamic collision 



is checked by finding out intersection between two faces or 
between a face and an edge. Brooks [8,9] characterized ’Free 
space’ as an union of generalized cones. A col 1 ision- free 
path for convex polygonal bodies through space littered with 
obstacle polygons can be got by maximizing the distance of 
closest approach to an obstacle. The method is based on 
characterizing the volume swept by a body as it is translated 
and rotated as a generalized cone while determining the 
conditions under which it is a subset of another cone. 

Chien [10] utilized the concept of state space 
and rotational mapping graphs to determine collision- free 
paths. The relationship between the position and the 
corresponding collision-free orientation of a moving object 
among obstacles is represented as a set of state space. 
This set is called ’Rotational happing Graph’. The 
problem of finding collision-free path for an object 
translating and rotating among obstacles is solved 
considering the connectivity of the ’ Rotational Happing 
Graph ’ . 

Hasegawa and Terasaki [11] solved the problem 
of collision avoidance by characterizing a ' Fully-Free 
Space’ where the robot can assume any configuration. Hence, 
the path has to be carefully chosen only in the vicinity of 
the obstacles, as any configuration is possible in the fully- 
free space. 


mentioned 


Host of the collision avoidance schemes 
emphasize a single robot within a fixed 


environment of stationary obstacles and achieve 


vari ed 


degrees of success. Of the few researchers who addressed 
the problem of path planning for multi-robots, Lee and Lee 
[12] proposed a method for determining potential 
collisions between the wrists of two robots while moving 
on straight line paths. Collisions between the wrists of the 
two robots is avoided by proper time scheduling. 

Nagata et.al [13] proposed a robot plan 
generation system which treats continuous state changes in 
time for multiple robots. The plan generation system 
consists of two subsystems - a fundamental planning subsystem 
for multiple robots and a system for detecting and avoiding 
mutual collisions of robots. In the strategy for avoiding 
collisions, the arms are modeled as cylinders. The 
collisions are avoided by the following three methods : 


(i) Uhen both 

the 

robots are to 

access a 

close 

point , only 

one 

robot acts at a 

time wh i 1 e 

the 


other robot remains idle. 

(ii) Collisions in movement are avoided by moving the 
arms in horizontal planes separated by a 
suitable distance. 

(iii) Uhen (i) and (ii) are not feasible, then an exact 
check for detecting collisions between the two arms 
is performed. 

The first subsystem obtains the fundamental 
sequence of actions while the second subsystem detects and 
avoids mutual collisions of robots. The final planning is 
performed by modifying the former subsystem through the 



results of the later subsystem. 

Most of the methods discussed above work 
satisfactorily where collision detection and avoidance are 
the primary concerns. But , when the requirement is an 
optimum collision-free path, then these methods are 
inadequate. Gilbert and Johnson [14,15] presented an 
algorithm which utilizes the concept of distance function 
to minimize the deviation from the center line path between 
obstacles. Suh and Shin [16] utilized a similar approach to 
determine a channel in the workspace of the robot, which 
contains at least one feasible path among the obstacles. 
A variational dynamic programming approach with minimum 
distance criterion is used to find the feasible path. 
However, very few researchers have tackled the 
problem of finding col 1 is ion- free minimum-time or 
minimum- energy paths for robots. 

Among the optimum path planning problems, the 
minimum-time path planning problem (MTPP problem) has 
received wide attention. The reason for this is that for 
economical use of robots, they should be operated to maximize 
the number of jobs performed in a given time. The first 
reported work dealing with MTTP problem is by Kahn and 
Roth [17]. Their formulation resulted in a highly nonlinear 
optimal control problem , which is difficult to solve. 
Luh and Ualker [18] and Luh and Lin [19] attempted to obtain 
minimum time along a given path by applying a combination of 
linear and nonlinear programming. These works assumed that 
the desired path is specified in terms of the two end points 
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and a set of intermediate points, called corner points. 

The robot's end effector is moved in straight line 
segments between the corner points with uniform velocity 
and in circular arcs around the corner points with 
uniform acceleration. They neglected the robot dynamics 
and put constraints on the maximum linear and angular 
velocities and accelerations. However, these extremal 
bounds depend on robot configuration, load and several 
other factors. It has been demonstrated that to accommodate 
the constraints on the accelerations, the velocities rarely 
reach their maximum specified values resulting in under 
utilization of the robot. [20]. Moreover, as the trajectory 
is generated in the Cartesian space, it is difficult to check 
the violation of actuator limits. 

One of the major break throughs in the time 
optimal control problem was reported by Hollerbach [21]. 
In this work, the time scaling property of manipulator 
dynamics which allowed modification of movement speed 
without recalculation of complete dynamics was proposed. Two 
elegant and somewhat similar solutions to the HTTP problem 
have been reported by Bobrow [22,23] and Shin and McKay [24]. 
The path to be followed by the robot is considered as a 
parameterized curve, and the optimal solutions are given by 
switching curves in phase plane. In [22], the actuator 
torque limits are functions only of robot positions and 
velocities. Shin and McKay on the other hand, considered the 
torque bounds as quadratic functions of the joint velocities. 
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A minimum-t ime trajectory is not always cost 
efficient. Koreover, the two methods mentioned above cannot 
take into consideration constraints on jerks, which may be 
required to prevent excessive wear on the mechanism. Shin 
and McKay [25] presented a method to tackle these 
limitations. This method, which solves a path-constrained- 
time-energy problem uses dynamic programming and is 
computationally expensive . 

The desired paths along which a robot must 
travel is usually specified in the Cartesian coordinate 
space and then converted, point by point, into the joint 
space of the robot. The minimum time of traversal depends on 
the geometric path selected. Shin and McKay [1] have 
presented a method for selecting near-minimum-time geometric 
path. 

All the methods presented assume that a 
collision-free path is already known, and the only 
requirement is to minimize the traversal time along this 
path. However, when a collision-free path is to be chosen 
such that a task can be performed in minimum time, then these 
methods are not useful. Furthermore, these methods rarely 
handle the minimum - time - energy problems. Patrikar [26] 
solved the problem of minimum-time path planning, as well 
as, the minimum-energy path planning in a novel fashion 
utilizing the ’Method of Local Variations’ [27,28] and the 
parameterization approach of Bobrov et.al [23]. The minimum- 
energy, as well as, the minimum-time path planning problems 
were solved in a computationally efficient and attractive 



manner- This method finds an optimal trajectory by 
perturbing a starting nominal trajectory. One of the 
shortcomings of this work is that it assumes that obstacles 
are specified by bounds in the joint space. This however, 
need not be the case as most of the obstacles are 
specified in the Cartesian coordinate space. 

1.3 Objective of the thesis : 

In this thesis, the method of Local Variations 
(MLV) has been employed to solve a variety of robot path 
planning problems. Methods for collision checking for 
static, as well as, dynamic obstacles are presented. These 
methods detect collision by computing the distance 
between the objects in the three dimensional space. The 
Euclidean metric is used because it conforms with the 
physical notion of distance and makes it invariant with 
respect to different choices for the origin and 
orientation of the coordinate systems. The collision 
checking method is incorporated into MLV and three types 
of robot path planning problems are proposed. 

First, a minimum- energy path is obtained in 
the presence of static obstacles. Then, minimum-energy paths 
for two robots working simultaneously is solved. The path 
planning for each robot treats the presence of the other 
robot as a dynamic obstacle to be avoided. In the third 
problem, specified the collision-free paths for two robots, 
the minimum time of traversal for each robot is determined. 

In addition to the above three problems, the 



problem of selecting a near-mini mum- time- energy geometric 
path for a robot is also proposed. 

1.4 Breakdown of Chapters : 

Chapter 2 deals with ' Co 1 1 i s i on- f r ee 
minimum- energy path planning 9 for a single robot. In this 
chapter, the optimal control problem has been formulated 
using robot dynamics. A method for checking collisions 
with static obstacles is developed. The problem has been 
solved by application of MLV . Results obtained through 
digital computer simulations are presented to verify the 
problem formulation. 

Chapter 3 deals with 9 Collision - free 
minimum - energy paths ' for two robots. A method for 
detecting collisions between two robot arms is presented. 
Two different types of problem formulations are considered. 
In the first case, a minimum- energy path for one robot is 
obtained. Given this information, a minimum- energy path for 
the second robot is obtained while avoiding any 
collisions with the first robot. In the second case, 
minimum- energy paths for both the robots is obtained 
simultaneously. Numerical examples are solved using MLV 
and the results obtained through digital computer 
simulations are presented. 

Chapter 4 deals with ' Minimum - time path 
planning p for two robots. The parameterized robot dynamics 
is used to formulate this problem. The collision checking 
scheme developed in Chapter 3 is utilized for solving the 
minimum-time path planning problem for the two robots. 



12 


In Chapter 5, the formulations of Chapter 2 
as well as, Chapter 4 have been utilized to develop the 
formulation for obtaining near ~ minimum - time - energy 
geometric paths. Different starting trajectories are 
considered and some guidelines have been formulated for 
finding near-minimum - time - energy paths. 

Chapter 6 lists the various conclusions drawn 
through simulation studies and identifies areas where further 
work needs to be done. 



CHAPTER 2 


COLLISION-FREE MINIMUM-ENERGY PATH 
FOR A SINGLE ROBOT 

In an industrial environment where a robot is 
required to perform repetitive jobs, the problem of selecting 
an optimum path attains significance. Since the workspace of 
the robot may contian various obstacles, the path must be so 
chosen that the arm avoids collision with static obstacles 
present in the workspace. 

Given the initial and final end-effector 
configurations and the time of travesal , from the efficiency 
point of view, it is required to find a path along which teh 
energy consumption will be minimum. At the same time, the 
robot must avoid collision with any obstacle. This is called 
an obstacle-free minimum- energy path. 

Most of the path pllanning schemes deal with 
the problem of avoiding static obstacles in the workspace of 
the robot. Due to this, the problem is usually cpnverted 
into a geometric analysis problem for checking collisions. 
Ususally, most of the static obstacles are represented by 
regular polyhedra. Exact representation is often unnecessary 
and increases computation. For collision detection, the 
primary requisite is a simple representation of the 
manipulator which eases the job of collision checking. It is 
sufficient to assume that any collision can occur between an 
obstacle and the wrist or the third llink of the robot. To 
simplify the representation, the wrist is modeled as a sphere 
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and the third link is modeled as a cylinder. Collision 
detection is done by checking for actual intersection between 
the arm and the obstacle. This Chapter deals with collision- 
free minimum-energy-path-planning for a single robot using 
the method of Local Variations. 

2.1 Problem Formulation : 

Neglecting the actuator inertia, the torque 
acting on a manipulator joint is given as 

n n n 

uj. = E Djj qj + E E Hxjk <ij <3k + G j 

J=1 J=1 K=1 

i=l,. . . , n (2.1) 

where ’n' is the total number of joints. The first term 
after the equal to sign is the inertia related term, the 
second one is due to centrifugal and coriolis forces acting 
on the torque and the last one is due to gravitational 
forces. Defining 


*1 

1 

1 

= 4i 


(2.2 a) 

1 

*n 

= 



X n+ 1 

1 

= *1 


(2.2 b) 

1 

X2 n 

= <*n 



Eqn . 

(2.1) is 

rewritten 

as 


n 

n 

n 

U£ = 

E Dij 

1 = 1 

x j + E 

i = l 

^ H ijk Xj X k + Gj (2.3) 

i = l 


i = 1 , . . , n 



The state vector defined by eqn.(2.2) has ’n’ elements, i.e., 
the positions of each joint (q^). To minimize the energy 
along the path of travel a cost function is chosen as 


t £ 


t £ 


J = K 
E 1 


KE dt + K 


PE dt 


(2.4) 


0 


0 


where KE and PE are the kinetic and potential energies 
respectively. Kj and K£ are two weighting factors. It is 
assumed that the time of traversal tf is fixed and the 
initial and final configurations are known. 

The MLV is then employed to find the optimal 
trajectory and its associated control. To do that first the 
time interval [0,tf] is divided into N equally spaced 
subintervals such that 


t f = N6T 

An initial nominal trajectory X* (k) , (k=l , . . ,N) is considered. 
The incremental cost between 2 successive time instants is 
calculated as 


F 

1 


1 


K 

1 


f KE dt 


1-1 


1 


+ K 

2 


1 

PE dt 


1-1 


(2.5) 


If <5T is sufficiently small, then eqn.(2.5) can be 
approximated as 


F = K <5T KE I + K «5T PE I (2.6) 

1 1 k=l 2 k=l 
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The values of the cost functional in each of the subintervals 
is computed and stored. 

The optimal cost is obtained by successive 
perturbation of each of the components of the state vector. 
The variation step size is fixed for each state component. A 
variation in the state at K instant results in changes in 
position in that instant and changes in velocity and 
acceleration in the subsequent time Instants. After each 
variation, testa are performed to check whether 
(1) the state is admissible 

(li) the resulting position does not result in collision 

with any of the obstacles 

(iii) the control Inputs required to translate the states 
from X(k-l) to X(k+1) via X(k) are admissible 

(iv) the subinterval costs for the intervals k-1 to k+1 is 
leas than the cost before variation. 

If the above teats give positive results, then 
the original state at instant k Is replaced by the perturbed 
value of the state. 

The local variation operation is applied 
successively for all components of the state vector at all 
instants of time to complete one iteration. At the end of 
one iteration, a new nominal trajectory Is got, which is used 
as the starting trajectory for the next iteration. If at the 
end of any iteration there is no reduction In cost, then the 
perturbation step size Is reduced by 50% and the process 
continued. This process which ensures a reduction In overall 



coat is performed till a satisfactory convergence is reached. 

* * 

The resulting X (k) , (k=l , . . . ,N+1 ) and u (k) , (k=l , . . ,N) are 
the desired optimal state and control vectors respectively. 

2.2 Collision Avoidance Scheme : 

Consider a 6 DOF robot with all revolute 
joints. Since most manipulators are of wrist partitioned 
type, the robot can be decomposed into 2 parts - an arm and a 
wrist, each containing 3 DOF. [12] Again as most of the 

obstacles are specified in the Cartesian coordinates, a simple 
model of the robot is necessary to simplify geometric 
constraint checking. For this, the wrist is modeled as a 
sphere, and the third link modeled as a cylinder. This 

arrangement is shown in Fig 2.1. It is assumed that the 
robot is going to be placed in such a way that there can be 
no collision between the second link and the obstacle. 

The calculation of exact obstacle interference 
is computationally intensive. Hence, some simplification is 
made in obstacle representation. As most of the obstacles 
found in any workspace are of planar, cylindrical etc., the 
obstacles are represented by regular polyhedra. To simplify 
the problem still further, any static obstacle present is 
assumed to be in the form of a wall. The robot has to travel 
from one side of the wall to the other side. 

Referring to Fig 1, once the joint angles of 
the robot arm are known at any given time, the coordinates of 
points A and B can be calculated through forward kinematic 
relations. The obstacle Is shown in Fig 2.2. Suppose the 
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robot has to move from a point to a point P 2 each placed 
on either side of the wall (Refer Fie 2.2). Then it must 
avoid collisions with faces and F£. Thus after every 

local variation, potential collision between any of these two 
faces and the robot have to be checked. This is done in two 
stages. First a collision check with the wrist is made, and 
then if found collision free a collision check is made with 
the third link. The variation is accepted if both these 
collision checks are passed. 

2.2.1 Collision checks for the wrist : 

The collision check is easier for the wrist 
than for the last link. Suppose the face with which the 

collision check is currently made is given by PQRS, and the 

center of the sphere is a point A (see Fig 2.3). Then, the 

plane on which the face is lying is determined from the 

coordinates of any three of the vertices of PQRS. Let the 
equation of the plane be given by 

ax + by + cz + d = 0 (2.6) 

Next step is to find out the distance between the center of 
the sphere and this plane. Let (xl.yl.zl) be the coordinates 
of the center of the sphere. Then this distance is given by 

axl + byl+ czl+ d 

distance = (2.7) 

tT ( a* +b* +c* ) 

If this distance is less than or equal to the radius of the 
sphere ( d i stanc e 4 r s ), then a potential for collision exists. 
This is not sufficient to prove that collision will occur 



s 


A (x, Y, z>) 


Fig. 2.5 s Collision check for the wrist. 



B 


Fig. 2.4 J 


Collision check for the third link. 



because the face is contained in a finite portion of the 
plane. 

If a potential for collision exists, then the 
positive occurrence of collision must be checked. To do 
that, first point A is joined with any vertex of the, face, 
say point P. Then the line PA is projected onto the straight 
line joining PQ, i.e., one edge of the face., The same step 
is repeated for the line containing edge PS. Now, if the 
point A is inside the face, then the projected lengths will 
be less than the length of the edges, and the collision is 
going to occur if the following conditions hold. 


PA' + QA' 
PA" + QA' ' 


< PQ + 2r s 

< PS + 2r s 


( 2 . 8 ) 


where A' and A'' are the projections of point A on the lines 
PQ and PS respectively. 

2.2.2 Collision Check for the third link : 


Knowing the forward kinematic relations, the coordinates 
of points A and B are calculated which are the end points of 
the line passing through the center of cylinder (see Fig 2-4). 
Let (x'.y'.z') be any point on this line. Then the equation 
of this line is 

x'-xi y'-yi z'-zi 

= = = r '(2.9) 

1 m n 

where (xl,yl,zl) are the coordinates of point A, l,m,n are 


the direction cosines and r is a constant. Solving eqn (2.9) 
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x' = xl + lr 

y v = yl + mr (2.10) 

z' = zl + nr 

Again, the distance between the point (x',y',z') and a plane 
containing the face of interest is given by- 

ax' + by' + cz' + d 

distance = (2.11) 

>f(a*+b*+c* ) 

Let this distance be equal to the radius of 
the cylinder r c . Then substituting equation (2.10) in 
equation (2.11) r can be calculated which will lead to the 
determination of the coordinates (x',y',z'). The point 

(x',y',z') is at a distance of r c from the plane containing 
the face of interest. 

If the point (x',y',z') lies within the points 
A and B then there exists potential for collision. However, 
to be sure, it is to be ascertained whether the projection of 
this point lies within the face of interest. This is done 
using the method described earlier. 

2.3 Numerical Results : 

The collision checking scheme is incorporated 

into the method of local variations. This algorithm Is 
tested through digital computer simulation of PUMA 560 robot 
arm. The results obtained are discussed below. 


Example 

2.1 : 

The robot 

arm is 

to travel 

from 

a point 

(0.6, 0.2 

,0.3) 

to a point 

O 

l 

O 

.2,0.2) 

in 

the 

Cartesian 

space . 

Thes e 

two points 

are on 

either 

side 

of 

a static 



obstacle which is a wall. The obstacle is aligned along the 
X axis in the Cartesian space and has a length of 0.8 t, 


height 

of 

0.3 m and thickness of 0.01 m. 





The robot has to travel in a optimal 

path 

such 

that 

the 

cost of equation (2.4) is minimized. Hence 

the 

value 

of 

Is chosen as 15 and that of K 2 Is chosen 

as 

0.5. 

This 

is 

to ensure that the Kinetic Energy term 

and 

the 

Potential 

Energy term are of the same order. It 

is 

also 


assumed that any violation of torque bounds can occur only in 
first 3 joints. The basis for this assumption is that the 
wrist is rotationally invariant and hence detailed torque 
computation for the last three joints is not required. The 
torque bounds for the first 3 joints are taken as 

I u I i 200 Nm, I u I i 400 Nra, |u I i 200 Nm ; 

1 2 3' 

First a nominal feasible trajectory is chosen 
such that its projection on both XY and YZ planes is a 
semicircle. The robot is assumed to have left-above arm 
configuration. Fig 2.5 depicts the projection of the third 
link and the wrist on the YZ plane for some discrete points 
along the nominal trajectory. The maximum clearance above 
the obstacle is 0.25m. along the Z-axis. The cost of this 
trajectory is calculated as 119.83 units. 

The MLV is now applied and the projection of 
the third link and the wrist on YZ plane for some discrete 
points along the optimal trajectory is shown in Fig 2.6. The 
maximum clearance thus obtained is 0.1 m. , which is a 
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reduction of 0.15 in. in height. The total cost is reduced to 
82 units. The obstacle is now removed and MLV applied to 
find an optimal path for this case. The projection for 
different points is shown in Fig 2.6 with squares. It can be 
seen that the optimal trajectory passes directly through the 
area where the obstacle would have been. 

Fig 2.7 shows the projections of all the three 
trajectories on XY plane. It can be seen from this figure 
that the trajectory obtained for the obstacle removed case is 
a line with minimum curvature, while the optimal trajectory 
in presence of obstacle is a curved path so as to avoid 
colliding with obstacle. The behavior of these trajectories 
are evident from Fig 2.8 and Fig 2.9 which plot the joint 
angles for joints 2 and 3, respectively. It can be seen that 
the optimal joint trajectory in the presence of obstacle Is 
almost two straight lines drawn from two extreme points, 
while it is almost a straight line joining the two extreme 
points when the obstacle is absent. 

2.4 Conclusions : 

In this chapter a scheme for checking 
collisions of the robot are with the static obstacles in the 
workspace has been presented. This method had been 
incorporated into the MLV algorithm in order to obtain 
collision free minimum energy paths. The collision checking 
schemes presented are rather stringent. However, they reduce 
computational requirements. The algorithm is tested through 
digital computer simulations. 
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2.5 : Nominal trajectory of the arm in Y-Z plane. 
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Pig. 2.6 : Optimal trajectory of the arm in Y-3 plane. 




(1) Nominal trajectory 

( 2 ) Trajectory with obstacle 

( 3 ) Trajectory without obstacle 


Fig. 2.7 : Projec 


tions of the trajectories on X-Y plane. 







An Important point to be observed is that any 
variational method can converge to local optima. To overcome 
this problem, it may be necessary in practice to obtain 
several solutions with different starting trajectories before 
drawing any conclusion on any optimal path. 





CHAPTER 3. 


COLLISION-FREE MINIMUM-ENERGY PATH PLANNING 
FOR TUO ROBOTS. 

In any industrial environment, the jobs to be 
performed may involve too many processing steps or simply the 
job may be too complicated for a single robot to handle. In 
such circumstances it may be necessary to use two robots 
working in tandem in order to reduce bottlenecks and to 
increase productivity. This chapter deals with minimum 
energy path planning for two robots working simultaneously in 
the same workspace. 

Given the nature of the job to be performed by 
these two robots, say Robot 1 and Robot 2 , two different 
approaches are considered to solve the problem of minimizing 
the energy of each of these robots while simultaneously 
avoiding collision with the other. In the first case, it is 
assumed that Robot 1 has higher priority. This is true when 
there already exists a robot and another robot is introduced 
to aid this robot. In this case the problem is formulated to 
treat Robot 1 as a dynamic obstacle which has to be avoided 
when planning the motion of Robot 2. 

In the second case, it is assumed that the 
initial and final points on the trajectories of Robot 1 ,as 
well as , Robot 2 along with the traversal time of each arm 
is specified. Given this information and the knowledge of 
actuator limits, paths for Robot 1 and Robot 2 are to be 
planned such that the overall expenditure of energy is 
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minimized and collision between the two arms is avoided. 

3.1 Problem Formulation : 

For simplicity two identical robots are 
considered. This , however , need not be a restriction and 
any two different types of robots can be considered. 

The minimum - energy path planning problem is 
then formulated in two different ways. They are as defined 
below : 

Problem Formulation 1 : 

Find the minimum - energy path for Robot 1 
assuming that Robot 2 is not present in the workspace. The 
optimal trajectory thus obtained is given by 

Xi € R - 3 

and the optimal control is given by 


* 3 

y.i € R 

The cost function assumed is of the form 



(3.1a) 


where Kj^ and are constants and KE and PE the kinetic and 

potential energies of the robot. Then, given the initial and 
final configurations for Robot 2 and the time of traversal 

% it 

t £ 2 , find Xjj and U 2 by minimizing a coat function given by 


tfl 


t £ 2 


J = K 
2 21 


KE dt + K 
2 22 



(3.1b) 


0 


0 


where 


K 21 and K 22 are constants and KE 2 and PE 2 are the 



kinetic and potential energies respectively of Robot 2 under 
the following constraints 


x 2 n 

Xi = 0 

for all 0 

< t < max(t fl ,t f2 ) 

(3.2) 

* 

u 2 € 

* 



(3.3) 



To solve 

this problem, the method adopted 

in 

Chapter 2 is 

utilized. 

The state vector formulation is 

the 


same as eqn.(2.2) and eqn.(2.3) . The essential difference 
is that while collision checking in Chapter 2 was for static 
obstacles ,* here the collision checking is done for dynamic 
obstacles. After each variation of a component of the state 
vector, the new position of Robot 2 is calculated and knowing 
the position of Robot 1 at this instant, checks are made to 
find out if this new position of Robot 2 results in 
collision with Robot 1 at this instant. 

Problem Formulation 2 : 

Given the initial and final configurations of 

'k k 

Robot 1 and Robot 2 , find and Uj , i = l,2 by minimizing 

the following cost function 

J l,2 = Ji + J2 (3.4) 

subject to the constraints 
X ! * ( T ) 0 X 2 *(T) = 0 for 0 < t < max(t£ 1 ,t f2 ) 

Ui*(t) fl Ui(t) i = 1,2 

where the subscripts refer to the robot in consideration. 

To solve this problem, first an extended state 
vector is formed as given below. 



xl 


<111 


x2 = <121 
x3 = q 3i 

(3.5) 

x4 = qi2 
x5 = q 3 2 
x6 = q 3 2 

where the first subscript of the terms on the R.H.S. refers to 
the joint and the second subscript refers to the robot in 
consideration . 

The nominal trajectories for the two robots 
are constructed. The nominal trajectories must not violate 
any of the constraints on bounds or control inputs. Further, 
the trajectories chosen must be collision - free. Each of 
the trajectories is divided into ’N’ equal subintervals. The 
subinterval costs are calculated and stored. The overall 
starting cost is found by eqn.(3.4). 

The variation step size for each of the 
components of the state vector is chosen. The variation is 
then performed for all the six elements of the state vector 
at each of the N-l instants. After each variation, tests are 
performed to check whether 

(i) the states are admissible 

(ii) the resulting position does not cause collision 
with the other robot. 

(iii) the control inputs required to translate the 
states from X^Ck-l) to X£(k+1) via Xj(k); (1=1,2) 
are admissible. 

(iv) the sum of subinterval costs, for the intervals k-1 



to k+1 is less than the sum of the costs before 
the variation. 

If the above tests give positive results then 
the original state at instant k is replaced by the perturbed 
value of the state. This process is repeated iteratively 
till no further reduction in costs are obtained. Then the 
perturbation step size is halved and the process repeated. 
The algorithm is terminated when satisfactory convergence is 
obtained . 

3.2 Geometric constraint checking : 

To simplify geometric constraint checking, 
certain assumptions about the robot arm are made. It is 
assumed that collisions can occur between the combination of 
the wrists and the third links of the manipulators. As 
stated before, the wrist is modeled as a sphere and the third 
link is modeled as a cylinder. 

To determine whether a collision exists, it is 
only required to know the positions of the first three joints 
of each robot arm. The procedure for checking collisions is 
as follows. 

Refer to Fig. 3.1 which depicts a typical 
arrangement of the two arms. Once the joint positions of the 
two arms are known, the coordinates of the points A,B,C and D 
can be calculated by forward kinematic relations. 

Define the following quantities for the 
identical robots. 

1 C 


: length of each cylinder. 



:radius of each cylinder. 


r c 

r s 

collision 

(i) 

Cii) 

( i i i ) 

(iv) 

They are 


rradius of each sphere. 

The following cases are considered for 
det ect ion : 

collision between wrist of Robot 1 and wrist of 
Robot 2 . 

collision between wrist of Robot 2 and third link of 
Robot 1 . 

collision between wrist of Robot 1 and third link 
of Robot 2. 

collision between third link of Robot 1 and third 
link of Robot 2. 
discussed in detail below. 


3.2.1 Collision between the wrist and wrist : 

This is the simplest of the four cases. Let 
B(X2,y2> z 2) and CCx 3 .y 3 .z 3 ) be the centers of the wrists of 
robots 1 and 2 respectively. Then the distance BC is given 
as 

BC = [ (X 3 - x 2 )’ + (y 3 ~ y 2) 2 + (Z 3 ~ z 2 )* 3 (3.6) 

If BC i 2 R S 

then a collision between the two wrists is a definite 
occurrenc e . 

3.2.2 Collision between wrist of Robot 2 and the third link 
of Robot 1 is checked as follows: 

Let A(xj_,yi_,zi) and B(x2,y2. z 2) represent the 
end points of the axis of the cylinder of Robot 1. 



Similarly, C(X 3 ,y 3 ,Z 3 ) and DCx 4 .y 4 .z 4 ) represent the 
corresponding points for Robot 2. Let and l 2 ,m 2 ,n 2 

be the direction cosines of AB and CD respectively. Then, 

(x 2 - Xi) (y 2 “ Yl) (z 2 “ z l) 

1 1 — > ~ 1 " 5 — » 

1 C 1 c 1 c 


Cx 4 - x 3 ) Cy 4 ~ Y 3 ) Cz 4 - z 3 ) 

12 = ■■■■■ ; tn 2 = ; n 2 = — — — (3.7) 

1 c 1 c lc 

First, the shortest distance between the 
sphere and the axis of the cylinder is to be computed. If 
this distance is less than or equal to (r a + r c ) , and if the 
point of contact lies on the cylinder, then collision exists. 
Refer to Fig. 3. 2 where Fig. 3. 2a depicts the case where the 
shortest distance is less than or equal to (r c + r s ) and the 
point of contact lies on the cylinder. Here collision 
occurrence is positive. Fig. 3. 2b illustrates the case where 
'C'is at a distance less than or equal to (r 3 + r c ) but the 
point of contact does not lie on the cylinder. Here, 
collision does not occur. 

Let the equation of the line AB be 


(x - x*) (y - yi ) C z - zx) 

= = = r (3.8) 

ll n ± 

where l^.m^.n^ are calculated according to eqn. (3.7). From 
eqn.(3.8) any point on AB has to satisfy 


X 

= X! 

+ 

ll 

. r 

y 

= Yl 

+ 

mi 

. r 

z 

= z 1 

4. 

n l 

. r 







Since the shortest distance, ’CE’ is perpendicular to AB, 

1 lj + a nj + n nj = 0 (3.10) 

Where l,m,n are the direction cosines of CE. From eqn.(3.9) 
and (3.10) and since 

1 1 1 + m ]_ r + ni' = 1 

the value of ’r’ is 

r = l 1 (x 3 -x 1 ) + roi(y 3 -y 1 ) + ni(z 3 ~zi) (3.11) 

knowing r, the coordinates of E can be calculated from 
eqn.(3.9). Now the length of the straight line joining 
points C and E is 

| CE I = < [ (x - x )* + (y - y )* + (z - z ) l ] (3.12) 

3 3 3 

If CE S (r s + r c ), then a potential for collision exists and 
further checks are needed to test for positive occurrence of 
collision. To do that, join point C to points A and B and 
find the projection of AC and BC on AB. (similar to section 
2.2). These projections are given by 

Proj. AC = AE = ABS[(x 3 - x^lj^ + (y 3 - vO m l + < z 3 ' z l) n ll 

(3.13) 

Proj. BC = BE = ABS[(X 3 - X 2 )li + (Y 3 ~ Y 2 )m 1 + (z 3 - z 2 )n x ] 

(3.14) 

If ( AE + BE ) s AB + 2 r a , then collision will definitely 
occur . 

The collision between the wrist of Robot 1 and 
third link of Robot 2 is checked in a similar fashion. 



3.2.3 Collision between the third links of the two robots is 


checked as follows : 

Let P be any point on the straight line 
at a distance of 2 r c from AB . Since P lies on CD 
following equations are satisfied. 


X = X 3 

+ 

1 z c 


y = y 3 

+ 

m 2 r 

(3.15) 

z = z 3 

+ 

n 2 r 


wh er e 

(X 

,y,z) are the coordinates of P. 



Refer to Fig. 3. 3 . A collision will occur 


the following hold: 

(1) the point P lies on the segment CD and 
(ii) the foot of the perpendicular from P onto 
straight line AB lies on the segment AB. 

Let 

All = Projection of AP on AB 
BM = Projection of BP on AB . 

The distances AM and BM are given by 


I AM I = ABS[(x-x )1 + (y-y )m + (z-z )n )] 

11 11 11 

I BM I = ABS[ (x-x )1 + (y-y )m + (z-z )n )] 

‘ 2 1 2 1 2 1 

From eqns.3.15 and 3.16 

AM* = Tl* + 2 T1 T2 r + T2* r* 

AP* = T3 + T4 r + r* 


(3.16) 


(3.17) 


CD 

the 


if 

the 




Fig. 3.3 : Collision check for the third links 



where 

T1 = (X 3 - X 1 H 1 + (y 3 - 71 ) 01 ! + (z 3 - zi)ni 
T2 = lil 2 + «1®2 + n l n 2 

(3. IS) 

T3 = (x 3 - Xj)* + (y 3 - yi )‘ + (z 3 - Zl ) 

T4 = 2 [ (x 3 - x x ) 1 2 + (y 3 - yi)m 2 + ( z 3 " zi)n 2 ] 

Again from triangle APM, using Pythagoros Theorem, 

PM* = AP* - AM* 

=[ T3 + T4 r + r * ] - [Tl* + 2 T1 T2 r + T2* r* ) 

(3.19) 

The next step is to find P such that PM = 2 r c ; i.e. the 
point at which the two cylinders touch each other. Solving 
eqn.(3.19), the value of r which makes this feasible can be 
determined. Since eqn.(3.19) is a quadratic in r , a real 
value of r exists only if the discriminant is greater than or 
equals zero. If a real value of r exists, then a potential 
for collision exists and further checks are necessary to 
determine positive occurrence. 

Let ri and r 2 be the two real roots of r. 
Then for each r if l=l,2 find the points P and M. Collision 
occurs if both P and M lie within CD and AB respectively. 

3 . 3 Numerical results z_ 

The MLV algorithm is applied to the problem 
formulated in 3.1 and the results verified through digital 
computer simulations. The efficacy of the collision checking 
scheme is clearly brought out through the following examples. 



Example 3.1 ( Problem formulation 1 ): 


In this example Robot 1 is required to move 
from ( 0 . 5 , 0 . 2 , 1 . 2 ) to ( 0 . 6 , -0 . 2 , 1 . 1 ) in the Cartesian 
coordinate space and Robot 2 is required to move from 
(0 . 5 , -0 . 2 , 1 . 0 ) to ( 0 . 6 , 0 . 2 , 1 . 2 ) simultaneously. The 

traversal time for both the robots Is 1 second. Constant 
bounds on the input torques of both the robots are assumed. 
They are as given In Example 2.1 . Robot 1 is assumed to 
have higher priority. 

Initially, Robot 1 path Is planned using 
MLV algorithm with K-j^ and Kj 2 chosen as In Example 2.1. 
i.e. 15 and 0.5 respectively. The minimum cost for this path 
Is found to be 76.48195 units. 

.Given, this path, a minimum- energy path for 
Robot 2 is sought. An initial feasible trajectory Is chosen 
which avoids colliding with with Robot 1 traveling in its 
optimum path. This path Is such that its projection on X-Y 
plane is a semicircle and motion In the vertical plane Is In 
equal increments. The cost function chosen is Identical to 
that of Robot 1. 

The initial cost along the nominal trajectory 
of Robot 2 Is 142.4389 units. On application of MLV this was 
subsequently reduced to 108.2730 units. The optimal total 
cost of the two robots Is 184.7549 units. The X-y plots for 
the Initial and final trajectories of the two robots are 
shown in Figs. 3. 4 and 3.5 respectively. 



Example 3.2 (Problem formulation 2) : 



In this 

exampl e , 

the 

Initial and 

final 

configurations 

for both the robots 

are the same 

aa 

in 

example 3.1 

The coat 

function 

in this case is given 

by 


eqn.(3.4) with K^, » %2l aftc * ^22 as * n example 3.1- . 

The Initial feasible trajectories for Robot 1 
and Robot 2 have semicircular projections on X-Y plane and 
equal increments / decrements in the vertical plane. The 
initial cost was 119.4910 unit# for Robot 1 and 142.4389 
units for Robot 2. The overall cost being 262.9299 units. 

The I1LV algorithm was applied according to 
problem formulation 2. The minimized cost for Robot 1 was 
83.1579 units and for Robot 2 was 95.5453 units. The overall 
minimized cost was found to be 180.7032 units. The optimal 
paths for the two robots according to this problem 
formulation is shown in Fig. 3. 6 . The joint position plots 
of the two robots for the nominal trajectory, as well as, for 
examples 3.1 and 3.2 are shown in Figs. 3.7 - 3.12 . 

3.4 Conclusions : 

In this chapter, a method for checking 
and avoiding collisions with dynamic objects in the work 
space of the robot has been presented. This method has been 
adapted for planning collision-free paths for two robots. 
The efficacy of collision avoidance scheme is clearly brought 
out in Figs. 3. 5 and 3.6 . 

It Is found that the overall cost resulting 
from problem formulation 2 Is not much lower compared to the 
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cost resulting from problem formulation 1. This is because 
the cost for Robot 1 in example 2 is greater than the cost in 
example 1, as It Is not traveling along the optimum path in 
this case. This Increase in cost of Robot 1 partly offsets 
the reduction In cost of Robot 2 obtained in problem 
formulation 2. Thus, the above two formulations can be used 
to determine if it is necessary to replan the path for both 
the robots, when a robot is Introduced into the work space of 
an already existing robot. 



CHAPTER 4. 


COLLISION-FREE MINIMUM-TIME PATH 
FOR TWO ROBOTS. 

In this chapter, collision - free minimum-time 
trajectory planning for two robots using the method of Local 
Variations is presented. The problem formulations assume 
that the paths to be followed by the end - effectors of the 
two robots are specified ’a priori *. The traversal time for 
each robot is to be minimised, while simultaneously avoiding 
collision with the other robot. 

In the minimum - energy path planning problems 
of Chapters 2 and 3, the time of traversal was a .fixed 
parameter. In the minimum - time path planning problem 
however, this is not the case, and the optimal time Is to be 
calculated. Hence, the strategies of chapter 2 and 3 cannot 
be applied directly. To overcome this problem, the 

parameterization approach is used [23,25]. I 

The path specified in the Cartesian coordinate 
space is first converted to a path in joint coordinate space 
and is then expressed in terms of a single parameter. 
Collision checking scheme developed in Chapter 3 is used to ! 

check for collisions. An optimal solution is then obtained j 

| 

by applying the MLV algorithm in phase plane. The j 

formulation is similar to that of Shin and McKay [ 25 ] . The j 

only difference between the two methods being in the - 
optimization algorithm used. Shin and McKay used dynamic . 

programming as an optimization technique in phase plane. j 



However , when MLV Algorithm la uaed , the method becomes much 
simpler to apply, as well as, reduces computation and memory 
requirements [26]. 

4.1 Problem formulation : 


First the parameterization approach for a 
single robot is discussed. It is then extended for two robot 
case . 


In the minimum - time path planning problems, 
the cost function is expressed as 


T = 


tf 

* 

dt 

« 

0 


(4.1) 


where the final time tf is left free. Because of this reason 
time can no longer be treated as a stage variable. The 
parameterization approach has a dual advantage. It not only 
takes care of .the above mentioned difficulty but also reduces 
the dimensionality of the problem from 'n’ for a n joint 
manipulator to one. 

The parameterization is achieved by expressing 
the joint positions as functions of a single parameter 's'. 
It is assumed that the path of the robot does not retrace as 
s varies from zero to s nAX . 

Thus 

<U(«0 * f i(«) 0 i a i a mAX (4.2) 

£ or I B 1 1 * • # ft 

where the <H’® represent the joint positions of the robot 
along the path. The dynamic equations of motion of a *n ' 



joint manipulator given by eqn.(2.3) is reproduced here. 


n 

li = S D ij <*j 
j=l 


n n 
+ E E 
j = l k=l 


H ijk <lj <Jk 


+ Gj, 


i = 1 


»n 


Defining 

ds 

M = 

dt ... 

and expressing q, q, q in terms of 's' , the following 

equations are obtained. 


<U = M 

da 

d 2 f £ . d£ i 

q A * M + M 

da da 


(4.3) 


Therefore, the parameterized dynamics equation 

can be written as 
Mi =* Mj[ m + Of M 2 + Gi 


where 


n 

■ E 


Hi 

da 


(4.4) 


and 


Qi 


n 

m E *>ij 
3»1 




n n 


+ 


F E 
j-1 k=l 


H i jk 


df j df k 
da da 


(4.5) 


Given the paths of both the robots in joint apace, each 
robot's path and dynamics can be expressed in terms of a 
single parameter. If the motion of Robot 1 is characterized 
by a parameter si and the motion of Robot 2 ia characterized 



by parameter »2» then the state equations can be formulated as 


®1 = Ml 


(4.6) 


82 * M2 

Uii = Hi! Mi + On M 2 + G A1 

2 

u A 2 = H A 2 M2 + 0i2 M + G i2 


(4.7) 


for 1 = 1 , . . ,n 

where the second subscript gives the robot number. 
M , M, 0 and G are as defined in eqna.(4.3) - (4.5). 
The time required to move the manipulators from the 
configuration to final configuration is given by 


initial 


T2 * 


tf : 

1 tfi tf 



* dt J 1 

ss 

dt * 

— — ds * 



1 

i 

i 

dsi J 

0 

0 0 

t f2 



r 1 


* 


ds 



2 

1 

M2 


0 




ds 


Ml 


(4.8) 


(4.9) 


where T1 and T2 are the traversal times of Robot 1 and 

Robot 2, respectively .The overall cost is then given by 
T » K1*T1 + K2*T2 (4.10) 

where K1 and K2 are constants chosen depending on the 

relative weightings given to costs of Robot 1 and Robot 2. 

The bounds on torques are arbitrary functions of positions 
and velocity and are expressed as 

J»l,2® in (*l,2,Ml,2> * El, 2 * «l,2* m *C*l,2»Ml,2) (4.11) 

where u * [u* u tt ) T 

If XI (t) denotes the set of all points on the 



Robot 1 at tiro© T , And X2(t) denotes th© set of all points on 
Robot 2 at the sane instant, then the positional constraint 
can be stated as 


XI (t ) 0 X2(t) * 0 for all 0 S t i max( T1,T2 ) (4.12) 


The optimal control problem is to find uj^* and Ml 2* 80 

that T is minimum subject to the boundary conditions 


Ml,2<0) s Ml , 2° a » d 

Ml,2( s max) * **1,2* 


(4.13) 


and the constraints specified by eqns.(4.11) and (4.12) 

* 

Optimal control «.i f 2 can be found by 
minimizing the expression for T given by eqn.(4.7). Time can 
be minimized if the two robots travel at maximum acceleration 
or maximum deceleration. The ft - a plane being the phase 
plane, the optimal solution is a switching curve in this 
plane. 

The variable ft is directly proportional to the 
velocities of the joints. Hence.it is called Psuedo- 
velocity. The traversal time for each robot can be minimized 
if all the joints travel at their maximum velocities. This 
means that the traversal time can be minimized if the Psuedo- 
velocity is maximized. 

4.2 Solution to the Minimum - time problem using MLV : 

Given the paths to be followed by the two 
robots, the range t0 to s la , ax J and 1 0 to a 2max3 i« divided 
into N equal subintervals. The joint positions of the two 
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robot* ar« «cpr.««.d in t„r„a of the paraaetere Sl and , 2 aa 

4 iven in .,n.(4.2). I h . function* f , ^ fc> 

continue*. function, of . 2 or , 2 . tt lo . nt ,UL m t If they 

are defined at each of theee dlacrete points ., , k 

df i d* f j 1,2> 

(k=l,..,n). The quantities and L at each ’k' is 

2 dai o f 

evaluated and stored. Since the 3oint positions are known at 
diacete points on the path, the coefficients of the dynamical 
equations (i.e. H 1Jk , G k ) at each of these points are 

calculated. Using these values and eqn.(4.5) the 
parameterized robot dynamic coefficients 

(Mil.ni2.Qil.0i2.Gii and G i2 ; i =1,..,3 ) at each of these 

points for the two robots are calculated and stored. 


Nominal starting p - a curves for the two 
robots are selected. The curves must satisfy the boundary 
conditions stated in eqn.(4.13). The nominal m - a curves 
chosen must not violate the torque constraints or result in 
collision between the arms. The incremental coats 
associated with each subinterval for both the robots is 
calculated and stored. The total cost for each robot is the 
■urn of all the incremental costa for its path. The overall 
coat T is calculated as given by eqn. (4.10). The 
incremental cost for interval [k-1 to k ] is the time taken 
by the arm to move from configuration specified at k-1 to 
the configuration specified at k. It is calculated as 


I 

f 

i 

I 

i 

i 


t 


! 

I 


1 


! 



t = 
1 



J Ml 
®1 ,k-l 


a 2 ,k 

r 1 

t = da 

2 2 
J M2 
e 2 , k-1 


These integrals can be approximated as 


ti = 2 8! / ( Ml (k) - Mi (k-1 ) ) 

t 2 =2 a 2 / ( M2<*) -B2Ck-l) ) 


(4.14) 


(4.15) 


Variations are performed on each component of the state 

vector at each of the N- 1 time instants. After each 

variation, tests are performed to check if any reeduction in 
coat for the subintervals [ k-1 ,k+l ] has reeemlted. If 
reduction in cost is obtained, then further testass to check 
for torque violation and collisions are carried oout. If the 
tests give negative results, then the state at thaais instant 
is replaced by the perturbed state. The al__gorithm is 
terminated when satisfactory convergence is reach«**d [26 ]. 

4.3 Collision checking scheme: 

Starting with two nominal trajec tories for 
the two robots which are collision-free, does not ensure that 
the optimal path will remain collision-free i f the MLV 
algorithm is applied to each path independently. Tlhis can be 
clearly brought out by the following example. 



Example 4.1 Consider two robots whose paths are such that 
they intersect at some point X in the cartesian space ( as 
shown in Fift. 4.1). The robots move along this path but each 
robot passes through X at a different time thus avoiding 
collision. Consider ;the time instant ’T’when Robot 1 is at 
'P'. After time 6T Robot 1 passes through X while Robot 2 is 
at point 'R*. At time ’T+6T' they are still separated by 
sufficient distance to prevent collision . If Robot 2 
occupies point X at a time 'T+ iT’ ’ , Robot 1 would have 
moved to point 'V’ in this time. Thus , the nominal 
trajectories and the traversal time chosen result in a 
collision-free path. 

If, the time of traversal for each robot is 
scaled by the same factor ,then the collision-free nature of 
the trajectories remain invariant. But in general, the nature 
of the paths being traversed, as well as the payload of 
individual Robots, make this impossible. 

Consider the case where ,an application of 
MLV, there is no change in the path history of Robot 1 while 
Robot 2 is speeded up so that it passes through X at a time 
’T+5T’ . This results in collision at point X although the 
variation has occured at ’s’ corresponding to point R 
Thus, it is not sufficient to check for collision only at the 
instant where the variation has occured, but it is necessary 
to check for collisions till the time both the robots reach 


their final configurations. 



4.4 Numerical example : 


Consider the problem where the end - effector 
of Robot 1 is required to move from a point (0 . 5 , 0 . 5 , 1 . 0 ) to 
a point (0.8, -0.2, 0.8) in the Cartesian coordinate space. 
Simultaneously, Robot 2 is required to move from a point 
(0 . 72 , -0 . 2 , 1 . 0 ) to a point (0.9,0.42,0.9). Robot 2 is 
positioned at a distance 1.47 m along the X-axis of Robot 1. 
It is desired to minimize the traversal time for both the 
robots, while avoiding collision between the two arms. 

The nominal trajectories of both the robots 
are such that their projections on the X-Y plane are semi- 
circles, and motion In vertical plane is in equal 
increments / decrements. The nominal traversal time for each 
robot is taken as 1 second. Thus the overall cost is 2 
units. The torque bounds are as in example 2.1. The 
collision checking scheme developed in section 3.2 is used to 
check for collisions between the two arms. 

The nominal joint trajectories of both the 
robots are parameterized. k nominal p - s curve is chosen 
for both the robots. For convenience, the trajectories of 
both the robots are divided into 64 equal subintervals. 
Hence the time for each subinterval for both the robots is 
0.015625 seconds and ’ji’ Is 64 in all the subintervals. 

The problem is formulated as explained in 
sections 4.1 & 4.2 and the HLV algorithm applied. The 
optimal time for Robot 1 was 0.48854 seconds and for Robot 2 



was 0.32229 seconds. The overall cost being 0.81083 unite. 
Now the minimum time along the nominal trajectory is found 
for each robot when the other robot ie not present. In this 
case the minimum time for Robot 1 was found to be 0.25736 
seconds and for Robot 2 the minimum time obtained was 0.21377 
seconds . 

The optimal m - s curves for both the robots 
are shown in Figs. 4.2 and 4.3 . The torque plots for the 
first joint of both the robots are shown in Figs. 4.4 to 4.7. 
The three dimensional view of the motion of the two robots 
along the trajectory for both the nominal and optimal case 
are shown in Figs. 4.8 and 4.9 respectively. 

4.4 Conclusions : 

In this chapter, a method to minimize the 
traversal time for two robots working simultaneously in a 
workspace was presented. The collision checking scheme 
developed in Chapter 3 was incorporated in the MLV algorithm 


and the 

parameterization approach 

utilized to solve 

this 

problem. 






From the optimal ’m 

- s’ curves, it 

is 

seen 

that the 

psuedo-velocity Cm) for both the robots 

is 

high 

leading 

to reduction in traversal 

time. However, 

in 

the 


initial part of the trajectory the value of psuedo-velocity 
is lesser than the corresponding value if only one robot was 
functioning. Further, the figures clearly indicate that the 
value of psuedo-velocity is the same in both the cases, with 
end without obstacle, once the two arms cross each other. 
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Fig. 4.6 


Joint 1 -Optimal torque plot of Robot 2 


With ob-stacW- 



Fig. 4.7 s Joint 1 -Optimal torque plot of Robot 2. 




This 


fact is further reinforced by Figs. 4.4 and 4.7. 


The 


efficacy of 
out through 


the collision avoidance scheme is clearly brought 
Figs. 4.8 and 4.9 respectively. 


CHAPTER 5 

NEAR MINIMUM -TIME- ENERGY GEOMETRIC PATH 
FOR A SINGLE ROBOT 

In the present industrial environment where 
robotic manipulator!) are finding widespread use, optimum 
utilization of i obotn attains significant dimensions. Some 
of the important considerations are minimizing energy or 
maximizing npe«t.l <<f operation. Maximizing speed of operation 
results in 1 ou»i operation time and greater throughput . 
Minimizing energy results in reducing wear and tear of 
actuators as well as operational cost. But invariably, 
increasing the speed of operation entails greater consumption 
of energy . Thus , the requirement of minimum operating time 
and minimum energy are conflicting in nature . In general, 
the actuators are required to be used to their fullest extent 
while simultaneously, the wear and tear as well as energy 
consumption must be low. 

The importance of time-optimal control of 
robotic manipulators has generated a wide research in this 
area. But most of the methods published so far fall short of 
requirement when the objective is neither minimum-time nor 
minimum energy . but a combination of both. Most of the 
methods deal with minimizing time given a particular path, 
but when the problem is to a path along which the coat- 
comprising of tint# and energy is minimum, *t*n these methods 
are inadequate . 



5.1 


Problem Formulation : 


In the MLV algorithm for finding minimum- 
energy paths, time was treated as a fixed parameter and the 
joint angles were treated as stage variables(Chapter 2). In 
contrast, time was a free variable in the problem of finding 
minimum time along a given path(Chapter 4). For the problem 
of finding near-minimum- t ime- energy geometric path, the 
objective function is a combination of total energy of the 
system and the time of traversal from system initial point to 
the final point. To accommodate this type of objective 
functions, the algorithm has to be suitably modified. 

Given the initial configuration and final 

configuration of the robotic arm, a feasible trajectory and a 
nominal time of traversal is to be chosen first. The initial 
trajectory is divided into N subintervals and the subinterval 
costs computed and stored. In the first pass of the 

algorithm, variations in joint angles(state variables) are 
performed and a new trajectory is chosen such that the 
overall energy is lowered. This is similar to the method 
adopted in chapter 2. During this process, time is treated 

as a fixed parameter as variations In joint position do not 

affect the time but affects only the velocities and 
accelerations of individual joints. After each state 
variable is varied in all the subintervals, the resulting 
trajectory is par amet er is ed and MLV applied to it as in 
Chapter 4. Each variation in parameter affects the time 
Involved in that particular subinterval as well as the next 



subinterval . 


This variation in time alters the Kinetic 


Energy for that interval and the next interval. Hence, 
before accepting a variation, it is necessary to check for 

V 

reduced overall cost and not just reduction in time. 

Specified the initial point ( x i»y£»Zi) and 
final point (Xf.yf.Zf) through which the manipulator has to 
pass, first a nominal feasible trajectory is chosen. This 
trajectory is then divided into N+l equidistant points and 
the joint angles (di , <12 » ^3) a_t 6ac h of these points computed 
and stored. An initial traversal time is assumed, say ’T’ 
secs. The time involved in each subinterval is taken to be 
T /N seconds . 

In the first pass of MLV Qi,Q.Z’ ( ^3 are treated 
as state variables. The velocities and accelerations are 
calculated at each- of the N+l points by means of difference 
equations 

q £ [ k ] = C q i [ k ] - q £ [ k-1 ] ) /Time [ k] (5.1) 

Q £ [ k ] = (q£ [k] - q£[k-l])/Time[k] (5.2) 

for i = 1 , . . . , 3 . 

The MLV is then applied as in Chapter 2. and 
at each variation of q£ , checks are made to verify if this 
new q£ results in lowering of overall cost. If it does, and 
if the new q£ does not violate either the joint constraint or 
the torque constraint then q£ is replaced by this new value. 
At the end of this process, when q£,q2>d3 have been varied In 
all the N subintervals, a new trajectory is available which 
is closer to the minimum- energy trajectory compared to the 
nominal trajectory. This trajectory is now parameterised . 


This is achieved by expressing the joint 


angles as a function of a parameter ’s’ . The trajectory is 
traced out as s varies from 0 to s^. 

<H — fj.O) 0 £ s i s max (5.3) 

for i = 

Then and q^ can be expressed in terms of s» as 


df 


ds 


df 


<U 




ds 

dff 


dt 


ds 


ds 


M + 




ds 


(5.4) 


(5.5) 


where ji = ds/dt . 

The dynamical equation for a ’n’ 
Lagrange-Euler form is given as 


joint manipulator in 


n n n 

ui = E Djj q? + E E Hij k qj q k + G£ 
j=l j=l k=l 

for i= 1 , . .n. 

substituting eqns . (5.4) and (5.5) in eqn. (5.6) 

uj[ = Mj[ fi + Of M 2 + Gi ; i = l,..,n 

df j 


(5.6) 


where 


n 

E 

J = 1 


D 


1 j 


ds 


n n 

Qi = ^ ^ijk 

j=l k=l 


df 


j d£ k 


n 

E D 


d 2 f 


ds 


ds 


j = i 


i j 


(5.7) 

(5.8) 

(5.9) 


ds 


Now the problem reduced to that of finding jj. 
and pi for each interval such that the overall cost is 
reduced. Now, time for each subinterval is given by 
2 / (p. [ k ] [ k- 1 ] ) . Hence varying ' jj. ’ affects the time for that 



subinterval as well as the next, which in turn affect the KE. 
The problem , however, is to correlate changes in energy to 


variations in m • This is done as follows. 



Since 

»i. 

Qi 

. Gi 

are already 

calculated 

and 

stored the 

eas i es t 

way 

is 

to 

use these 

values to find 

a 

relationship 

betve en 

9 M ' 

and 

the 

energy . 

For 

simplicity 

1 et 


n be 3. From eqn. (5.8) 
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+ D 
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ds 
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dfi 

+(Di2 + D2i) 

da 


d f 2 d f i 

+ (D 13 +D 31 ) 

da ds 


df 3 

da 


df 2 

(Di 3 +D 3 i) 

ds 


df 3 

da 


(5.10) 


df df df i 


r ds , 

12 3 

M rrr 4* M , + M ^ 



12 3 

ds da ds J 


- dt ■ 


= D 11 CqiD'+D 2 2 (q2)' +D 33 (d3^ * + ( D 12 +D 21 ) 
+(D23+D 32 ) q2Q3 


q q q 

12 3 


D 

D 

D 

11 

12 


D 

D 

D 

21 

22 


D 

D 

D 

31 

32 




qiq 2 + (Di3 + D 3 i ) qiq 3 


=V T DV=K.E. ( 5 - 11 ) 

Since L.H.S. of (5.11) is precalculated, the variation in ’ fi ’ 
can be easily calculated without taking recourse to 
calculation of velocity, D,H,G matrices etc,. The overall 
problem formulation is as shown below. 


5.1.1 


Near-Minimum-Time-Energy Geometric Path 


Planning 


Problem: 



The 

probl era 

can 

now 

be stated as follows. 

Given 


(initial) 

at 

t = 

0 

(5.12) 


= Qi 

(final) 

at 

t = 

T 


for 1=1,.., n and the dynamic eqns . given by (5.6) or (5.7), 

k k 

find the optimal inputs u^ , traversal time T and the time 

k k k 

history of the variables » Q2 * q 3 such that the cost 

function given by (5.13) is minimized subject to the bounds 
expressed in eqn . (5.12). 

T T 


C 



+ K 

2 


( (3 V T D V + (3 PE ) dt 
1 2 


0 


(5.13) 


where K^ f K2» 3i , P 2 are constants, V is the joint velocity 
vector and PE is the potential energy of the arm. 


5.3 Numerical examples: 

Consider the case when the robot manipulator 
tip is required tomove from a point (0.5,0.2,1.23 to a point 
(0 . 6 , -0 . 2 , 1 . 0 3 in the Cartesian coordinate space in near 
minimum - time - energy path. For simplicity, the path is 
assumed to be obstacle free. The manipulator arm is required 
to operate within the bounds on the input torques. These 
bounds are as chosen in example 2.1 . 

Since the problem is to find the optimum 
energy and time of traversal, the cost function selected is 
of the form given in eqn. 5.13 . and K 2 are taken as 100 
and 0.25 and 3i and 32 are taken as 15 and 0.5 respectively. 



Equal weightage is assumed for the energy of the robot, as 
well as, for the time of traversal. Thus the choice of 
and K 2 is made such that both the terms being integrated in 
eqn. 5.13 are approximately of the same magnitude. This 
ensures equal priority to reduction in time, as well as, 
energy of the robot's path. 

In order to study the dependence of the 
optimal path on the starting trajectory, various starting 
trajectories have been tried out. They are as follows : 


case( i ) : 


case( i i ) : 


case( i i i ) 


case( iv) 


Circular trajectory: The initial path chosen is 
such that its projection on X~ Y plane is a semi- 
circle and movement in the vertical plane is in 
equal decrements. 

Startin£ trajectory close to minimum energy path: 
The nominal trajectory chosen in case(i) is 
minimized by application of the MLV algorithm. 
The criterion function is same as that chosen in 
example 2.1 . The algorithm was terminated when 

the resulting trajectory was sufficiently close to 
the optimum. The resulting trajectory was 
minimized according to problem formulated in 5.1 . 

The minimum- energy path is first-obtained and then 
the procedure outlined in 5.1 applied to obtain 
the near - minimum - time - energy geometric path. 
The nominal path was assumed to a straight line 
joining the initial and the final points of the 


robot's desired motion. 

















In all the above cases, the nominal time for the robot to 
move from the initial to the final configuration was assumed 
to be 1 second. The results obtained are presented in table 
5.1 . The nominal trajectories of the first three joints and 
the optimal trajectories obtained fom case(iii) are shown in 
Figs. 5.1 to 5.3 . The optimal jj, - s plot is shown in 
Fig. 5.4 . The torque plots of the first three joints are 
shown in Figs. 5.5 to 5.7 . 

From the figures it is evident that while the 
joint trajectories for the near - minimum - time - energy 
geometric path are lines of minimum curvature close to the 
corresponding minimum - energy path, the torque plots show 
that the joint torques are bang bang in nature to ensure high 
velocities for individual joints. This results in a high 
speed of traversal and thus lower traversal time. Further, 
it can be seen that the final energy of the robot is higher 
than the energy along the nominal path. This is because the 
time of traversal has been reduced. Initially, the traversal 
time was 1 second but the optimal traversal time is 0.23 
seconds. This reduction in time causes increase in the 
velocity of the arm which leads to increase in kinetic 
energy. 


5.4 Conclusions : 

In this chapter the near - minimum - time 
energy problem was formulated. The problem formulation was 
tested with different nominal trajectories. From the results 
it is clear that the optimal trajectory will be in the 
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if the 


neighbourhood of the minimum energy path. Further 
starting trajectory is far off from the optimum, 
resulting trajectory may converge to a local optim 
in order to obtain a near - minimum - time - energy 
path it is desirable to choose the minimum 


then the 
Thus, 
geometric 
energy 


trajectory as the nominal trajectory. 



CHAPTER 6. 


GENERAL CONCLUSIONS AND SCOPE FOR FURTHER UORK. 

In this thesis, methods for detecting collisions 
between different objects has been developed. This has been 
incorporated in the MLV algorithm and different types of robot 
path planning problems solved. This chapter lists out the 
conclusions drawn from the simulation studies. Scope for further 
work is also discussed. 

Conclusions : 

(1) The efficacy of collision detection schemes for the static, 
as well as- the dynamic, obstacles has been clearly 
demonstrated by means of examples. The geometric checking 
scheme is powerful and can handle obstacles of different 
types . 

(2) Minimum - Time and Minimum - Energy path planning for two 
robots has been successfully attempted for the first time. 

(3) Method to formulate and solve minimum - time - energy path 
planning problems has been presented and guidelines for 
choosing such paths laid down. 

SCOPE FOR FURTHER UORK : 

Although simplification in the robot arm structure 
is made for the purpose of collision detection, the computation 
necessary is still large. This can be overcome by checking for 



<30 


collision only at a discrete number of points on the trajectory. 
The minimum distance between the arm and the obstacle can be 
stored at each variation. At the next variation, checks are made 
only if this distance is lesser than a predetermined value. If 
the incremental step size is small, then the resulting variation 
in position of the arm will not be much and collision checks can 
be made only if the arms are very close toeach other. Another 
approach to the problem can be by mapping the arm and the obstacle 
positions in the joint space. If a computationally efficient 
method is devised, then the collsion checks can be easily 
performed by checking if the joint vector after any variation lies 
outside the feasible domain of the joint space. 

Uhen collision - free optimal paths for two robots 
are desired, then collision checking computation can be 
substantially reduced by determining a subset of the total 
workspace where collisions can occur. Only in this subset, 
detailed collision checks are performed. The algorithm can also 
be suitably modified to take into cosideration payload 
and parameter uncertainities. 
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